#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <unistd.h>
#include <X11/Xlib.h> // 包含 X11 库的头文件

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pcl_filter_example");
    ros::NodeHandle nh;

    char cwd[1024];
    if (getcwd(cwd, sizeof(cwd)) != NULL)
    {
        std::cout << "Current working directory: " << cwd << std::endl;
    }
    else
    {
        std::cerr << "Error getting current working directory" << std::endl;
        return 1;
    }

    // 读取点云文件
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("./pointsData/Horse.pcd", *cloud) == -1)
    {
        PCL_ERROR("Couldn't read file 'Horse.pcd'\n");
        return -1;
    }

    // 创建直通滤波器
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");  // 指定过滤的字段为z坐标
    pass.setFilterLimits(0.0, 1.0);  // 指定过滤范围为0到1之间的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pass.filter(*filtered_cloud);

    // 可视化点云
    pcl::visualization::CloudViewer viewer("PointCloud Viewer");

    viewer.showCloud(filtered_cloud);

    while (!viewer.wasStopped())
    {
        // 等待直到窗口关闭
        viewer.spinOnce(100);
    }

    return 0;
}
